using System;
using System.Collections.Generic;
using System.Drawing;
using System.Text;
using Diversity.Maths;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// The main class needed to do Grid Slam. Helps reduce uncertainty in map making.
    /// </summary>
    public class GridSlam
    {
        private Robot _robot;
        private TrigUtils _trigUtils;

        /// <summary>
        /// The robot.
        /// </summary>
        public Robot Robot
        {
            get { return _robot; }
            set { _robot = value; }
        }
        private int _numParticles;

        /// <summary>
        /// The number of particles.
        /// </summary>
        public int NumParticles
        {
            get { return _numParticles; }
            set { _numParticles = value; }
        }
        private Pose2D[] _states;
        private Map[] _maps;
        private double[] _weights;
        private double[] _weightsAggregate;
        private double _informationReduction = 1;

        /// <summary>
        /// Compresses the results of comparisons between the latest readings and each particle's map.
        /// This has the effect of making resampling less severe, allowing greater diversity.
        /// Should be a number above zero and less than or equal to one. One means no compression.
        /// </summary>
        public double InformationReduction
        {
            get { return _informationReduction; }
            set { _informationReduction = value; }
        }
        private bool _allowResampling = true;

        /// <summary>
        /// If false, no resampling will occur.
        /// </summary>
        public bool AllowResampling
        {
            get { return _allowResampling; }
            set { _allowResampling = value; }
        }

        private bool _DoStepWhenVelocityZero = false;

        /// <summary>
        /// If false, no resampling will happen when velocity is zero
        /// </summary>
        public bool DoStepWhenVelocityZero
        {
            get { return _DoStepWhenVelocityZero; }
            set { _DoStepWhenVelocityZero = value; }
        }

       // private ProbabilityUtils ProbabilityUtils;
        private int _step = 0;

        /// <summary>
        /// The current step.
        /// </summary>
        public int Step
        {
            get { return _step; }
            set { _step = value; }
        }

        private bool _conditionSamplesOnMap = false;

        /// <summary>
        /// If true, this will attempt to kill velocity estimates that would leave a particle
        /// in an area considered to be occupied.
        /// </summary>
        public bool ConditionSamplesOnMap
        {
            get { return _conditionSamplesOnMap; }
            set { _conditionSamplesOnMap = value; }
        }

        /// <summary>
        /// Creates the GridSlam object. Helps reduce uncertainty in map making.
        /// </summary>
        /// <param name="robot">A definition of the robot and its sensors.</param>
        /// <param name="numParticles">The number of particles you want to use to represent the hypothesis.</param>
        public GridSlam(Robot robot, int numParticles)
        {
            _trigUtils = new TrigUtils();
            _robot = robot;
            _numParticles = numParticles;
           // ProbabilityUtils = new ProbabilityUtils();
            PrepareParticles();
        }

        private void PrepareParticles()
        {
            _step = 0;
            _states = new Pose2D[_numParticles];
            _maps = new Map[_numParticles];
            _weights = new double[_numParticles];
            _weightsAggregate = new double[_numParticles];

            // MAKE PARTICLES
            for (int i = 0; i < _numParticles; i++)
            {
                _states[i] = (Pose2D)_robot.Pose2DCurrent.Clone();
                _maps[i] = new Map(_robot.GlobalMap.Width, _robot.GlobalMap.Height, _robot.GlobalMap.Threshold, _robot.GlobalMap.Scale);
                _weights[i] = 1.0 / _numParticles;
            }
            Console.Out.WriteLine("GridSlam: Created fresh states and maps");
        }


        /// <summary>
        /// Adds readings to the local map, bypassing slam.
        /// </summary>
        /// <param name="state">The robot's state</param>
        /// <param name="readings">An array of posed distance readings</param>
        public void AddRawReadings(Pose2D state, DistanceReading[] readings)
        {
            _robot.LocalMap.AddReadings(state,readings);
        }

        /// <summary>
        /// The main step in GridSLam using an odometry model.
        /// </summary>
        /// <param name="state">The latest estimated state.</param>
        /// <param name="distanceReadings">An array of posed distance readings.</param>
        public void DoStep(Pose2D state, DistanceReading[] distanceReadings)
        {
           

            // hack: calculate a velocity based on the difference between the _lastState and _state
            // TODO use the proper odometry model


           
                /*
                double dist = _robot.LastState > state;

                double angle = state.Theta - _robot.LastState.Theta; // hack

                //double angle = _robot.LastState%state;// Math.Atan2(state.Y - _robot.LastState.Y, state.X - _robot.LastState.X);
                //}
                if (angle < -Math.PI)
                {
                    angle += Math.PI*2;
                }
                if (angle > Math.PI)
                {
                    angle -= Math.PI*2;
                }
                Velocity v = new Velocity(dist, angle);
                if (v.Translation != 0)
                {
                    Console.Out.WriteLine("%%%%%%%%%%%%%%% Wahey! " + v);
                }
                _robot.LastState = (State)state.Clone();
                _robot.Move(v);
                DoStep(v, distanceReadings);
                */

            if (!DoStepWhenVelocityZero && _robot.Pose2DLast.IsAlmostEqualTo(state))
                {
                    _robot.LocalMap.AddReadings(_robot.Pose2DCurrent, distanceReadings);
                    _robot.LocalMap.ClearRobotPosition(_robot.Pose2DCurrent);
                    UpdateAllMaps(distanceReadings);         
                } 
                else
                {
                    _robot.Move(state);
                    _robot.LocalMap.AddReadings(_robot.Pose2DCurrent, distanceReadings);
                    _robot.LocalMap.ClearRobotPosition(_robot.Pose2DCurrent);
                    SlamStep(distanceReadings);        
                }

            _step++;
        }

        /// <summary>
        /// The main step in Grid Slam using a velocity model.
        /// </summary>
        /// <param name="velocity">The estimated velocity.</param>
        /// <param name="distanceReadings">An array of posed distance readings.</param>
        public void DoStep(Velocity velocity, DistanceReading[] distanceReadings)
        {
           // _robot.Move(velocity);
           
            _robot.LocalMap.AddReadings(_robot.Pose2DCurrent, distanceReadings);
            _robot.LocalMap.ClearRobotPosition(_robot.Pose2DCurrent);

            if (!DoStepWhenVelocityZero && velocity.Translation == 0 && velocity.Rotation == 0)
            {
               UpdateAllMaps(distanceReadings);
               // Console.Out.WriteLine("GridSlam: No velocity, so no resampling");
            } else
            {
                SlamStep(distanceReadings);
            }

            _step++;
        }

        private void UpdateAllMaps(DistanceReading[] distanceReadings)
        {
            for (int i = 0; i < _maps.Length; i++)
            {
                _maps[i].AddReadings(_states[i], distanceReadings);
            }
        }

        private void SlamStep(DistanceReading[] distanceReadings)
        {
            

            // take a copy of the weights before messing with them
            double[] mapWeights = new double[_weights.Length];

            // Loop around all the particles, moving them and calculating their likelyhood
            for (int i = 0; i < _numParticles; i++)
            {
                // TODO Could be avoided
                Pose2D oldState = (Pose2D)_states[i].Clone();
                bool isAllowedByMap = false;

                // Get Sample applies the Motion model with added noise
                int mapFailCount = 0;
                while (!isAllowedByMap && mapFailCount < 10)
                {
                    mapFailCount++;
                    _states[i] = _robot.MotionModel.GetSample(oldState);
                    isAllowedByMap = (_conditionSamplesOnMap)
                                         ? _robot.IsValidState(_maps[i], _states[i])
                                         : true;
                } // this might allow a dead particle through, but it should get filtered out.
                if (mapFailCount == 10)
                {
                    Console.Out.WriteLine("Bad move");
                }

                // Before updating the particle's local map, get a scan
                DistanceReading[] stateReadings = _robot.GetDistanceReadings(_maps[i], _states[i], 0);

                // Using the robot's beam model, calculate the correspondance between the real
                // readings and the particle's scan
                mapWeights[i] = _robot.CompareDistanceReadings(stateReadings, distanceReadings);

                if (mapWeights[i] == 0)
                {
                    Console.Out.WriteLine("Zero prob scan");
                }
            }

            mapWeights = ProbabilityUtils.Normalize(mapWeights, Normalization.MaxToOne);


            // do the information reduction
            for (int i = 0; i < mapWeights.Length; i++)
            {
                mapWeights[i] = Math.Pow(mapWeights[i], _informationReduction);
            }

            mapWeights = ProbabilityUtils.Normalize(mapWeights, Normalization.IntegralToOne);

            // A generic low variance resampler, that works with a normalized array of weights.
            // The indexes can be used to determine which particles are needed,
            // therefore which maps need updating.
            int[] indexes = ProbabilityUtils.ReSample(mapWeights);

            // now step through the particles an update the weights using baysian approach
            _weights = mapWeights; // ProbabilityUtils.BayesUpdate(weights, mapWeights);

            if (!_allowResampling)
            {
                // no resampling, so apply the readings to each map
                for (int i = 0; i < _maps.Length; i++)
                {
                    _maps[i].AddReadings(_states[i], distanceReadings);
                }
                Console.Out.WriteLine("No resampling");
            }
            else
            {
                #region Create resampled sets

                List<int> AllreadyCopied = new List<int>();
                Pose2D[] newStates = new Pose2D[indexes.Length];
                Map[] newMaps = new Map[indexes.Length];
                double[] newAggregates = new double[indexes.Length];
                double[] newWeights = new double[indexes.Length];
                int o = 0;
                foreach (int eee in indexes)
                {
                    if (AllreadyCopied.Contains(eee))
                    {
                        // alas this particle needs cloning.
                        newStates[o] = (Pose2D)_states[eee].Clone();
                        newMaps[o] = (Map)_maps[eee].Clone();
                        newWeights[o] = _weights[eee];
                        newAggregates[o] = _weightsAggregate[eee];

                        // keeps a track of lost particles (i.e. duplicates)
                    }
                    else
                    {
                        // assignment is much faster than cloning
                        AllreadyCopied.Add(eee);
                        newStates[o] = _states[eee];
                        _maps[eee].AddReadings(_states[eee], distanceReadings);
                        _maps[eee].ClearRobotPosition(_states[eee]);
                        newMaps[o] = _maps[eee];
                        newWeights[o] = _weights[eee];
                        newAggregates[o] = _weightsAggregate[eee];
                    }
                    o++;
                }
                _states = newStates;
                _maps = newMaps;
                _weights = newWeights;
                _weightsAggregate = newAggregates;

                #endregion
            }

            // add to the aggregate weights
            for (int i = 0; i < _weights.Length; i++)
            {
                _weightsAggregate[i] += _weights[i];
            }
            ;
        }

        /// <summary>
        /// Gets the Global Map as an image.
        /// </summary>
        /// <returns>A bitmap representing occupancy.</returns>
        public Image GetGlobalImage()
        {
            Bitmap b = new Bitmap(_robot.GlobalMap.Width, _robot.GlobalMap.Height);
            b = (Bitmap)ProbabilityUtils.DrawLogOddsDistribution(b, _robot.GlobalMap.Occupancy);
            return b;
        }

        /// <summary>
        /// Gets the Local Map as an image.
        /// </summary>
        /// <returns>A bitmap representing occupancy.</returns>
        public Image GetLocalImage()
        {
            Bitmap b = new Bitmap(_robot.LocalMap.Width, _robot.LocalMap.Height);
            b = (Bitmap)ProbabilityUtils.DrawLogOddsDistribution(b, _robot.LocalMap.Occupancy);
            return b;
        }

        /// <summary>
        /// Gets the best Map from all particles.
        /// </summary>
        /// <returns>A bitmap represnting occupancy.</returns>
        public Image GetBestImage()
        {
            Bitmap img = new Bitmap(_robot.GlobalMap.Width, _robot.GlobalMap.Height);
           // if (double.IsNaN(_weightsAggregate[0]))
          //  {
           //     Console.Out.WriteLine("GridSlam: Problem with the aggregate weights: NaN");
           //     return (Bitmap)ProbabilityUtils.DrawLogOddsDistribution(img, _maps[0].Occupancy);
           // }
//else
          //  {
                double maxAggregate = double.MinValue;
                int indexOfMax = 0;

                for (int i = 0; i < _numParticles; i++)
                {
                    if (_weightsAggregate[i] > maxAggregate)
                    {
                        maxAggregate = _weightsAggregate[i];
                        indexOfMax = i;
                    }
                }

                if (maxAggregate == 0)
                {
                    Console.Out.Write("Max aggregate zero: " + maxAggregate + " " + indexOfMax);
                }
                Bitmap tmpBest = (Bitmap)ProbabilityUtils.DrawLogOddsDistribution(img, _maps[indexOfMax].Occupancy);
                //  pictureBoxSLAMBest.Refresh();

                /*
                // prepare to draw the map of particles
                double[,] poses = new double[_robot.GlobalMap.Width,_robot.GlobalMap.Height];
                foreach (State state in _states)
                {
                    int x = (int) Math.Round(state.X/_robot.GlobalMap.Scale);
                    int y = (int) Math.Round(state.Y/_robot.GlobalMap.Scale);
                    if (x > 0 && y > 0 && x < _robot.GlobalMap.Width && y < _robot.GlobalMap.Height)
                    {
                        poses[x, y] = 1;
                    }
                }
                 * */
                ProbabilityUtils.DrawParticles(ref tmpBest, _states, _robot.GlobalMap.Scale);

                return tmpBest;
           // }
        }

        /// <summary>
        /// Finds the best state estimate from all particles.
        /// </summary>
        /// <returns>The best state.</returns>
        public Pose2D GetBestState()
        {
            double maxAggregate = double.MinValue;
            int indexOfMax = 0;

            for (int i = 0; i < _numParticles; i++)
            {
                if (_weightsAggregate[i] > maxAggregate)
                {
                    maxAggregate = _weightsAggregate[i];
                    indexOfMax = i;
                }
            }

            return _states[indexOfMax];
        }
    }
}
